import DobotDllType as dType


if __name__ == "__main__":
    # Load Dll and get the CDLL object
    api = dType.load()
    
    # Connect Dobot
    state = dType.ConnectDobot(api, "COM9", 115200)[0]
    if state != dType.DobotConnect.DobotConnect_NoError:
        print("Failed to connect to Dobot")
        exit(1)
    dType.SetQueuedCmdClear(api)
    # Set HOME parameters
    dType.SetHOMEParams(api,250, 0, 20,-30,isQueued=1)
    last=dType.SetHOMECmd(api,temp=0, isQueued=1)[0]
    dType.SetQueuedCmdStartExec(api)
    while last > dType.GetQueuedCmdCurrentIndex(api)[0]:
        dType.dSleep(100)
    # Get current pose
    pose = dType.GetPose(api)
    print(f"Current Pose: X={pose[0]}, Y={pose[1]}, Z={pose[2]}, R={pose[3]}")
    
    # Disconnect Dobot
    dType.DisconnectDobot(api)